﻿using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Presentation;
using Microsoft.SPOT.Presentation.Controls;
using Microsoft.SPOT.Presentation.Media;

using GT = Gadgeteer;
using GTM = Gadgeteer.Modules;
using Gadgeteer.Modules.GHIElectronics;

using GTI = Gadgeteer.Interfaces;
using System.Threading;
using System.Text;
using Gadgeteer.Modules.Seeed;

namespace GadgeteerRobot
{
    public partial class Program
    {
        public GTI.Serial serial;
         GT.Interfaces.AnalogInput Left_IRRange ;
         GT.Interfaces.AnalogInput Right_IRRange;
        void ProgramStarted()
        {
            GT.Socket socket = GT.Socket.GetSocket(UART.ExtenderSocketNumber, true, null, null);
            serial = new GTI.Serial(socket, 115200, GTI.Serial.SerialParity.None, GTI.Serial.SerialStopBits.One, 8, GTI.Serial.HardwareFlowControl.NotRequired, null);
            GTI.DigitalOutput resetkey = new GTI.DigitalOutput(socket, GT.Socket.Pin.Three, false, null);
            resetkey.Write(true);
            serial.ReadTimeout = 50;
            serial.Open(); 
            serial.LineReceived += new GTI.Serial.LineReceivedEventHandler(serial_LineReceived);
            Left_IRRange = this.IRSensor.SetupAnalogInput(GT.Socket.Pin.Four);
            Right_IRRange = this.IRSensor.SetupAnalogInput(GT.Socket.Pin.Five);
            Left_IRRange.Active = true;
            Right_IRRange.Active = true;
            
            oledDisplay.SimpleGraphics.DisplayRectangle(GT.Color.Yellow, 1, GT.Color.Black, 10, 10, 128 - 20, 128 - 20);
            oledDisplay.SimpleGraphics.DisplayText(".Net Gadgeteer", Resources.GetFont(Resources.FontResources.NinaB), GT.Color.Red, 12, 40);
            oledDisplay.SimpleGraphics.DisplayText("Robot", Resources.GetFont(Resources.FontResources.NinaB), GT.Color.Red, 40, 60); 
            GT.Timer timer = new GT.Timer(100);
            timer.Tick += new GT.Timer.TickEventHandler(timer_Tick);
            timer.Start();
             
            // Do one-time tasks here
            Debug.Print("Program Started");
        }

        void timer_Tick(GT.Timer timer)
        {
            byte[] rx_byte = new byte[80];
  
            byte tx = 0; ;
            byte rx = 0; ;

            Debug.Print("Left : " + Left_IRRange.ReadProportion().ToString() + " Right : " + Right_IRRange.ReadProportion().ToString());

            if (Left_IRRange.ReadProportion() < .4)
            {
                if (serial.IsOpen)
                {
                    tx = 45;//just a random speed I chose. Needs to be tuned. Even better, create a service endpoint and change remotely via wifi, zigbee, radio, etc
                    serial.Write(142);
                    serial.Write(tx);
                }
            }
            else
            {
                if (serial.IsOpen)
                {
                    tx = 20; //just a random speed I chose. Needs to be tuned. 
                    serial.Write(140);
                    serial.Write(tx);
                    Thread.Sleep(600);
                }
            }
            if (Right_IRRange.ReadProportion() < .4)
            {
                if (serial.IsOpen)
                {
                    tx = 30;//just a random speed I chose. Needs to be tuned. 
                    serial.Write(136); //specifies direction and motor (motor A or B)
                    serial.Write(tx);
                }
            }
            else
            {
                if (serial.IsOpen)
                {
                    tx = 20;//just a random speed I chose. Needs to be tuned.
                    serial.Write(138);//specifies direction and motor (motor A or B)
                    serial.Write(tx);
                    Thread.Sleep(700);//let the motor run for 700 ms before checking again.
                }
            }
            Thread.Sleep(50);//can probably be removed
        }

        void serial_LineReceived(GTI.Serial sender, string line)
        {
        }
    }
}
